#include <iostream>
#include <cassert>
#include <cmath>
#include "resize.hpp"
using namespace std;
const float M_PI = 3.141592653f;

float lanczos (int N, float d)
{
    return sinf(M_PI*d)/(M_PI*d) * sinf(M_PI*d/(float)N)/(M_PI*d/(float)N);
}

float clamp (float a, float b, float value)
{
    return std::max(a, std::min(b, value));
}

/**
 *
 */
void resize_by_lanczos (const unsigned char* src_pixels, int src_width, int src_height, 
                        unsigned char* dst_pixels, int dst_width, int dst_height,
                        int bpp, int n)
{
    float  scale_x = dst_width  / (float)src_width;
    float  scale_y = dst_height / (float)src_height;
    float* color   = new float[bpp];

    for (int dst_y = 0; dst_y < dst_height; dst_y++) {
        for (int dst_x = 0; dst_x < dst_width; dst_x++) {

            float src_x  = (dst_x+0.5f)/scale_x;
            float src_y  = (dst_y+0.5f)/scale_y;
            int   src0_x = (int)src_x;
            int   src0_y = (int)src_y;
            float w_sum  = 0;

            for (int i = 0; i < bpp; i++) {
                color[i] = 0;
            }

            for (int i = -(n-1); i <= n; i++) {
                for (int j = -(n-1); j <= n; j++) {
                    int xx = src0_x + i;
                    int yy = src0_y + j;

                    xx = (xx < 0) ? 0 : (xx > src_width -1) ? src_width -1 : xx;
                    yy = (yy < 0) ? 0 : (yy > src_height-1) ? src_height-1 : yy;

                    float dx = (xx+0.5f) - src_x;
                    float dy = (yy+0.5f) - src_y;
                    float d  = sqrtf (dx*dx + dy*dy);

                    float w  = (d == 0) ? 1.f : (d <= n) ? lanczos(n, d) : 0.f;
                    
                    for (int i = 0; i < bpp; i++) {
                        color[i] += w*src_pixels[yy*src_width*bpp + xx*bpp + i];
                    }

                    w_sum += w;
                }
            }

            for (int i = 0; i < bpp; i++) {
                color[i] = clamp (0, 255, color[i]/w_sum);
            }

            for (int i = 0; i < bpp; i++) {
                dst_pixels[dst_y*dst_width*bpp + dst_x*bpp + i] = (unsigned char)color[i];
            }
        }
    }


}


int pow_of_2 (int a)
{
    if (!(a & (a-1))) {
        return a;
    }

    for (int i = 0; i < sizeof(int)*8; i++) {
        if (a == 1) {
            return 1<<i;
        }
        a = a >> 1;
    }

    return 0;
}